from camera_model import Camera
from pylab import *
import runpy

def compute_fundamental(x1,x2):
    """
    从view1和view2形成的点对计算基本矩阵F，
    输入数据：x1为image1上的齐次坐标点，x2为image2上的齐次坐标点，是3xN的数组。
    使用规范化的8点算法，将输入合成一个矩阵，每一个点对合成一行，其每一行为：
        [x'*x, x'*y, x', y'*x, y'*y, y', x, y, 1]
    """
    n=x1.shape[1]   #有多少个数据点
    if x2.shape[1]!=n:
        raise ValueError("Number of points don't match.")

    A=zeros((n,9))
    for i in range(n):
        A[i]=[x1[0,i]*x2[0,i], x1[0,i]*x2[1,i], x1[0,i]*x2[2,i],
              x1[1,i]*x2[0,i], x1[1,i]*x2[1,i], x1[1,i]*x2[2,i],
              x1[2,i]*x2[0,i], x1[2,i]*x2[1,i], x1[2,i]*x2[2,i] ]
    #计算最小二乘解，使用SVD分解
    U,S,V=linalg.svd(A)
    F=V[-1].reshape(3,3)    #最小奇异值对应的特征向量即为Ax=0的最小二乘解

    #对F进行约束，det(F)=0
    U,S,V=linalg.svd(F)
    S[2]=0;
    F=dot(U,dot(diag(S),V))

    return F


def compute_epipole(F):
    """
    根据F计算极点，Fe1=0
    """
    U,S,V=linalg.svd(F)
    e=V[-1]
    return e/e[2]

def plot_epipolar_line(im,F,x,epipole=None,show_epipole=True):
    """
    绘制image1中点x对应在image2中的极线(l'=Fx)。
    x为image1中的点，im为image2
    """
    m,n=im.shape[:2]    #图像长宽
    line=dot(F,x)       #得到极线
    t=linspace(0,n,100) #0到n之间的100个点
    lt=array([(line[0]*tt+line[2])/(-line[1]) for tt in t  ])

    ndx=(lt>=0) & (lt<m)
    plot(t[ndx],lt[ndx],linewidth=2)

    if show_epipole:
        if epipole is None:
            epipole = compute_epipole(F)
        plot(epipole[0]/epipole[2],epipole[1]/epipole[2],'r*')


#载入2D及3D数据
file=r'Metron1Data/load_vggdata.py'
with open(file,'rb') as f:
    exec(f.read())
#execfile('Metron1Data/load_vggdata.py')

#得到角点的索引，要保证第1列和第2列的索引都是有效的
ndx=(corr[:,0]>=0) & (corr[:,1]>=0)
#使用bool作为索引时，只收集其中对应下标为True的元素
x1=points2D[0][:,corr[ndx,0]]
x1=vstack( (x1,ones(x1.shape[1])) ) #添加一行齐次坐标
x2=points2D[1][:,corr[ndx,1]]
x2=vstack( (x2,ones(x2.shape[1])) )

#计算F
F=compute_fundamental(x1,x2)
#计算极点
e=compute_epipole(F)

figure()
imshow(im1)
#单独绘制每条线
for i in range(5):
    plot_epipolar_line(im1,F,x2[:,i],e,True)
axis('off')

figure()
imshow(im2)
for i in range(5):
    plot(x2[0,i],x2[1,i],'o')
    axis('off')
show()